function [Y cache] = geodesic_best_new(clusters, SS, repetitions)

% fast memory efficient implementation of geodesic interpolation algorithm
% SS, the state sequence
% repetitions, the number of repetitions for each element in the state sequence.
% return cache containing variables that can be reused by geodesic_best_new_cached

% scalar variables
if nargin > 2
    S = constructFullSequence(SS, repetitions);
    T = sum(repetitions);
else
    S = SS;
    T = numel(S);
end
DIM = size(clusters.covars, 1) / 2;
Q = size(clusters.covars, 3);

% more variables
x = 1:DIM;
dx = DIM+x;

% compute inv_covars, if needed
if ~isfield(clusters, 'inv_covars')
    for i = 1:Q
        clusters.inv_covars(:, :, i) = inv(clusters.covars(:, :, i));
    end
end

% compute auxilliary variables, mu, dmu and K
for i=1:Q
    mu{i} = clusters.centres(i, x)';
    dmu{i} = clusters.centres(i, dx)';

    A{i} = clusters.inv_covars(x, x, i);
    B{i} = clusters.inv_covars(x, dx, i);
    C{i} = clusters.inv_covars(dx, x, i);
    D{i} = clusters.inv_covars(dx, dx, i);
end

% compute left hand side of equation 6
AA = zeros((T+1)*DIM, 3*DIM);
AA(1:DIM, DIM+(1:2*DIM)) = [  D{S(1)}, ...
                            -(C{S(1)} + D{S(1)})];
for t =1:T-1
    rows = t*DIM + (1:DIM);     cols = 1:3*DIM;
    AA(rows, cols) = [-(B{S(t)}   + D{S(t)}), ...
                        A{S(t)}   + B{S(t)} + C{S(t)} + D{S(t)} + D{S(t+1)}, ...
                      -(C{S(t+1)} + D{S(t+1)})];
end
AA(T*DIM + (1:DIM), 1:2*DIM) = [-(B{S(end)} + D{S(end)}), ...
                                  A{S(end)} + B{S(end)} + C{S(end)} + D{S(end)}];

BB = zeros(1, (T+1)*DIM);
BB(1:DIM) = -C{S(1)} * mu{S(1)}(:) - D{S(1)} * dmu{S(1)}(:);
for t=1:T-1
    BB(t*DIM+(1:DIM)) =   (A{S(t)}   + C{S(t)}) * mu{S(t)}(:) ...
                        + (B{S(t)}   + D{S(t)}) * dmu{S(t)}(:) ...
                        -  C{S(t+1)} * mu{S(t+1)}(:) ...
                        -  D{S(t+1)} * dmu{S(t+1)}(:);
end
BB(T*DIM+(1:DIM)) = (A{S(end)} + C{S(end)}) * mu{S(end)}(:) + (B{S(end)} + D{S(end)}) * dmu{S(end)}(:);



Y = reshape(solveBlockTridiagonal(AA, BB', DIM), DIM, T+1)';



% setup cache
cache.clusters = clusters;
cache.SS = SS;
if nargin > 2
    cache.repetitions = repetitions;
else
    cache.repeititons = ones(size(cache.SS));
end
cache.A = A;
cache.B = B;
cache.C = C;
cache.D = D;
cache.mu = mu;
cache.dmu = dmu;

cache.AA = AA;
cache.BB = BB;
